/**

This file is part of MaCI/GIMnet.

MaCI/GIMnet is free software: you can redistribute it and/or modify it 
under the terms of the GNU Lesser General Public License as published 
by the Free Software Foundation, either version 3 of the License, or 
(at your option) any later version.

MaCI/GIMnet is distributed in the hope that it will be useful, but WITHOUT 
ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or 
FITNESS FOR A PARTICULAR PURPOSE.  See the GNU Lesser General Public 
License for more details.

You should have received a copy of the GNU Lesser General Public 
License along with GIMnet. (See COPYING.LESSER) If not, see 
<http://www.gnu.org/licenses/>.

**/
/**
* Connects to Position and Ranging Servers and builds a sensor based map, based on Occupancy Grid
* The Class Aims to provide many usefull functions...
* 
* KESKEN:
- Bumberin lisäys
- Getterit
*/
#ifndef MACI_OCCU_MAPPER_H_
#define MACI_OCCU_MAPPER_H_

#include <stdio.h>
#include <stdlib.h>
#include <string>
#include <vector>
#include "PositionClient.hpp" ///< For receiving pose from the GIMnet
#include "RangingClient.hpp"  ///< For receiving rangedata from the GIMnet
#include "RangingServer.hpp"  ///< For sending motion detection filtered rangedata
#include "thread.hpp"         ///< ThreadFunction
#include "sync.hpp"
#include "ImageServer.hpp"    ///< In case image server is activated
#include "ImageContainer.hpp"
#include "MapServer.hpp"      ///< in case the mapserver is activated
#include "JPEGEncoder.hpp"    ///< For Jpeg encoding 
#include "RGBImage.h"         ///< container to ease the manipulation on rgb data
#include "TOccupancyGrid.h"


#define GIMI_CLIENT_API (20000)
#include "gimi.h"             ///< Le' GIMI

using namespace gim;

/**
* Special structure to enable drawing to the image
* The structure specifies a metric position and color as a RGB value
*/
struct MaCIOccuMapperDrawPixel{
		float x; ///< absolute position with respect to grid origin
		float y;  ///< absolute position with respect to grid origin
		unsigned char r;
		unsigned char g;
		unsigned char b;
		 
		MaCIOccuMapperDrawPixel(){
				set(0,0,0,0,0);
		}
		MaCIOccuMapperDrawPixel(float x_, float y_, uchar r_, uchar g_, uchar b_){
				x = x_; y=y_; r=r_;g=g_; b=b_;
		}
		void set(float x_, float y_, uchar r_, uchar g_, uchar b_){
				x = x_; y=y_; r=r_;g=g_; b=b_;
		}
		void setRGB(uchar r_, uchar g_, uchar b_){
				r=r_;g=g_; b=b_;
		}
};


class CMaCIOccuMapper : public CThread, public CSync{
		public:
				/**
				* Constructor with All possible "switches"
				* @param gimnetap The network name of the Hub host (e.g. "localhost")
				* @param gimnetapport The tcp port number of the Hub (e.g. 50002)
				* @param gimnetname The gimnet name of this module (if left blank="" anonymous name will be created)
				* @param macigroup The MaCI Group name of this module, as well as the position and ranging sources
				* @param positionserverinstancename The instance name of the position server (e.g. "Motion")
				* @param rangingserverinstancename The instance name of ranging server (e.g. "Laser")
				* @param occumapsize Size of the map that is maintained in meters
				* @param occumapresolution The size of one cell in meters
				* @param edgethreshold if closer than this to the map edge, then the map will be centered 
				* @param numberofscanpoints Some number that should be more than maximum 
				*																				number of ranges arriving from the server
				* @param max_laser_dist Maximum allowed distance for laser measurement
				* @param min_laser_dist Minimum allowed laser distance
				*/
				CMaCIOccuMapper(std::string gimnetap, 
												int gimnetapport, 
												std::string gimnetname,
												std::string macigroup,
												std::string positionserverinstancename,
												std::string rangingserverinstancename,
												double occumapsize,
												double occumapresolution,
												float edgethreshold,
												int numberofscanpoints,
												float max_laser_dist,
												float min_laser_dist
											 );
				/**
				 * Constructor with initilized GIMI and MaCI service list
				 * @param *Gptr initialized and connected GIMI ptr
				 * @param *&services_ MaCI service list 
				 * @param macigroup The MaCI Group name of this module, as well as the position and ranging sources
				 * @param positionserverinstancename The instance name of the position server (e.g. "Motion")
				 * @param rangingserverinstancename The instance name of ranging server (e.g. "Laser")
				 * @param occumapsize Size of the map that is maintained in meters
				 * @param occumapresolution The size of one cell in meters
				 * @param edgethreshold if closer than this to the map edge, then the map will be centered 
				 * @param numberofscanpoints Some number that should be more than maximum 
				 *																				number of ranges arriving from the server
				 * @param max_laser_dist Maximum allowed distance for laser measurement
				 * @param min_laser_dist Minimum allowed laser distance
				 */
				CMaCIOccuMapper(gimi::GIMI *Gptr,
												MaCI::MaCICtrl::TServiceEntryArray &services_,
												std::string macigroup,
												std::string positionserverinstancename,
												std::string rangingserverinstancename,
												double occumapsize,
												double occumapresolution,
												float edgethreshold,
												int numberofscanpoints,
												float max_laser_dist,
												float min_laser_dist
											 );
				/**
				 * Constructor with initilized GIMI,  MaCI service list and with a settings file
				 * @param *Gptr initialized and connected GIMI ptr
				 * @param *&services_ MaCI service list 
				 * @param macigroup The MaCI Group name of this module, as well as the position and ranging sources
				 * @param positionserverinstancename The instance name of the position server (e.g. "Motion")
				 * @param rangingserverinstancename The instance name of ranging server (e.g. "Laser")
				 * @param settings_file_name
				 */
				CMaCIOccuMapper(gimi::GIMI *Gptr,
												MaCI::MaCICtrl::TServiceEntryArray &services_,
												std::string macigroup,
												std::string positionserverinstancename,
												std::string rangingserverinstancename,
												std::string settings_file_name);
				
				
				~CMaCIOccuMapper();
				
				/**
				* Returns the latest pose received from the positionClient
				*/
				void getCurrentPose(Grid::pose &pose);
				
				/**
				* Return the latest pose and laser measurements
				**/
				void getCurrentMeasurements(Grid::pose &pose, Grid::TScan &grid_scan);
				
				/**
				* Return pointer to occupancy grid
				* NOTE:: The data is constantly updated, so use with care
				**/
				TOccupancyGrid *getMapPointer(){return O;}
				/**
				* "cut" the low and high probability values
				* @param low_prob limit for the lower probability
				* @param high_prob limit for the high probability
				*/
				void blurMap(float low_prob, float high_prob);				
				/**
				* Get a submap around certain position
				* @param **data pointer to where the data is copied (malloced or if NULL, will be allocated)
				* @param pos the metric absolute position around which the map is taken from
				* @param lNx number of pixels in submap in x-dir
				* @param lNy number of pixels in submap in y-dir
				*/
				void getSubmap(float **data, Grid::TCoord2D pos, int lNx, int lNy);
				
				/**
				 * Get a submap around robots position
				 * @param **data pointer to where the data is copied (malloced or if NULL, will be allocated)
				 * @param lNx number of pixels in submap in x-dir
				 * @param lNy number of pixels in submap in y-dir
				 */
				void getSubmap(float **data, int lNx, int lNy);
				
				/**
				* Get distance transform of (lNx x lNy) size of block around the robot
				* The pTh is used in binary image thresholding (larger prob equals 1 lower equals 0)
				* @param **data pointer to where the data is copied (malloced or if NULL, will be allocated)
				* @param lNx number of pixels in submap in x-dir
				* @param lNy number of pixels in submap in y-dir
				* @param pTh probablity threshold (lower equals 0 higher equals 1)
				**/
				void getDistanceTransform(float **data, int lNx, int lNy, float pTh);
				
				/**
				 * Get distance transform of (lNx x lNy) size of block around the robot
				 * The pTh is used in binary image thresholding (larger prob equals 1 lower equals 0)
				 * @param **data pointer to where the data is copied (malloced or if NULL, will be allocated)
				 * @param lNx number of pixels in submap in x-dir
				 * @param lNy number of pixels in submap in y-dir
				 * @param pTh probablity threshold (lower equals 0 higher equals 1)
				 * @param *Occu The data pointer of submap is set to Occupancy Grid and latest measurement added to the map
				 **/
				void getDistanceTransform(float **data, int lNx, int lNy, float pTh,TOccupancyGrid *Occu);
				
				
				
				/**
				* Activates the Image Server. The server plots the map to image (RGB or GRAYSCALE), 
				* and additionally displays the laser and pose of the robot in the map
				* @param instanceName The instance name of the image server
				* @param useRGBimage if true the map is copied as RGB, else as grayscale (to save some space)
				* @param visualizeLaser if true the laser  will be visualized also
				* @param laserPixSize The size of the visulized laser hit. If zero a pixel is plotted, if > 0 a circle is plotted
				* @param visualizePose if true the pose will will be visualized also
				* @param robotRad The dimension of the robot
				* @return  true if success, else false
				*/
				bool activateImageServer(std::string instanceName, 
																 bool useRGBimage=false, 
																 bool visualizeLaser=false,
																 float laserPixSize=0,
																 bool visualizePose=false, 
																 float robotRad=0.25);
				
				/**
				* Activates the Map Servers
				* The map server provides the mapped map as an occupancy grid
				* @param instanceName The instance name of the server
				* @param sendLocalMaps if true the map server will send local maps
				* @param lmapsize How big local map is read around the robot
				* @return true is success
				**/
				bool activateMapServer(std::string instanceName,bool sendLocalMaps=false, float lmapsize=5.0);
				
				
				/**
				* Activates the Ranging Server
				* The ranging server provides the filtered scan where only 
				* reflections from moving objects are present.
				* @param instanceName The instance name of the server
				* @return true is success
				**/
				bool activateRangingServer(std::string instanceName);
					
				/**
				* Set the frequency of image sending 
				* The number is "How many scans is skipped before sending an image"
				* 
				*/
				void setImageSendingFrequency(int freq){imageSendingFrequency = freq;}
				
				void setMapSendingFrequency(int freq){mapSendingFrequency = freq;}
				/**
				* Stop executing the thread
				*/
				void stopOccuMapperExecution();
				
				/***************************************************************
				* Drawing related methods
				****************************************************************/				
				/**
				* adds a pixel to be drawn
				**/
				void addDrawPixel(MaCIOccuMapperDrawPixel pix);
				void addDrawPixel(float x, float y, uchar r, uchar g, uchar b);
				void clearDrawPixelVector();
				
				/**
				* Converts an absolute pose to relative pose with respect to 
				* map
				*/
				Grid::pose toRelativePose(Grid::pose &abs_pose);
				
				/**
				 * Converts an absolute pose to relative pose with respect to 
				 * map
				 */
				Grid::TCoord2D toRelativePose(Grid::TCoord2D &abs_pos);
				/**
				 * Converts a relative pose to absolute pose with respect to 
				 * map
				 */
				Grid::TCoord2D toAbsolutePose(Grid::TCoord2D &rel_pos);
				
				
				/**
				 * Converts a relative pose to absolute pose with respect to 
				 * map
				 */
				Grid::pose toAbsolutePose(Grid::pose &rel_pose);
				
				/**
				* Resets the map to 0.5
				*/
				void Reset();
								
				/**
				* Mapping thread
				*/
				int ThreadFunction(int num);
				
				float getMaxLaserDistance(){return max_laser_distance;}
		private:
				double occu_map_size;       ///< size of the occupancy grid in meters
				double occu_map_resolution; ///< Resolution of the occupancy Grid
				TOccupancyGrid *O;              ///< Occupancy Grid
				Grid::TScan grid_scan;
				float max_laser_distance;
				float min_laser_distance;
				int enough_scan_points;    ///< ranging server should not provide more points than this
				float EdgeThreshold;        ///< if closer than this to the edge, then the map will be centered
				
				bool threadRunning;        ///< flag for thread, false will terminate the thread
				
				bool imageServerActivated; ///< Flag to indicate if the image server is activated
				bool mapServerActivated;   ///< Flag 
				
				Grid::pose current_pose;   ///< current pose from the robot
				Grid::pose relative_pose;  ///< relative pose of the robot with respect to the map
				Grid::pose grid_pose;      ///< the absolute pose of the grid origin
				
				/**
				* These are required to access the Position and Ranging data
				*/
				std::string gimnetAP;
				int gimnetAPPort;
				std::string gimnetName;
				std::string maciGroup;
				gimi::GIMI *gimi_ptr;
				gimi::GIMI *gimi_ptr2; ///< For Ranging Server to avoid conflicts with the Ranging Client
				std::string positionServerInstanceName;
				std::string rangingServerInstanceName;
				
				MaCI::MaCICtrl::TServiceEntryArray services; ///< the services
				MaCI::MaCICtrl::CMaCICtrlServer *MSC; ///< MaCI ctrl server ptr
				MaCI::MaCICtrl::CMaCICtrlServer *MSC2; ///< MaCI ctrl server ptr
				
				MaCI::Position::CPositionClient *positionClient;
				std::vector< MaCI::Ranging::CRangingClient* > rangingClients;
				 
				
				/** 
				* Image server Variables
				*/
				MaCI::Image::CImageServer *imageServer;     ///< Image server instance
				std::string imageServerInstanceName;        ///< Instance Name for the server 
				bool isRGBenabled;                          ///< Enable RGB (if disabled gray scale is used)
				bool laserVisualizationEnabled;             ///< Enable/Disable laser plotting
				bool poseEnabled;
				float robotRadius;
				float laserHitSize; ///< the size of the plotted pixel/circle
				MaCI::Image::CImageData imageData;
				char *picbuf;
				RGBUtil::RGBImage *rgbi;
				CJPEGEncoder encoder;
				int imageSendingFrequency; ///< how many scans is skipped before sending new image
				
				std::vector<MaCIOccuMapperDrawPixel> drawpixels;
				
				/**
				* Map Server Variables
				*/
				MaCI::Map::CMapServer *mapServer;
				std::string mapServerInstanceName;
				MaCI::Map::CMapData mapData;
				MaCI::Map::CMapData gmapData;
				bool sendLocalMaps;
				int localMapSize;
				float *lmapdata;
				int mapSendingFrequency;
				unsigned char *localCharMap;
				unsigned char *globalCharMap;

				/**
				* Ranging Server Variables
				*/
				std::string OwnRangingServerInstanceName;
				bool bSendMotionRanges;
				MaCI::Ranging::CRangingServer *rangingServer; 
				
				/**
				* Initialize the variables and start the thread
				*/
				void initializeInternalVariables();
				
				/**
				 * Initialize the variables and start the thread
				 * Uses Initialized MaCI service list 
				 */
				void initializeInternalVariables(MaCI::MaCICtrl::TServiceEntryArray &services_);
				
				/**
				* Initializes the given variables (for documentation see constructor)
				**/
				void initializeExternalVariables(std::string gimnetap, 
																				 int gimnetapport, 
																				 std::string gimnetname,
																				 std::string macigroup,
																				 std::string positionserverinstancename,
																				 std::string rangingserverinstancename,
																				 double occumapsize,
																				 double occumapresolution,
																				 float edgethreshold,
																				 int numberofscanpoints,
																				float max_laser_dist,
																				float min_laser_dist);
				/**
				* Sends the map data
				* @param &pos pose of the grid
				*/
				bool sendMap(Grid::pose &pos,Grid::pose &rel_pos);
				
				/**
				 * Send the current occupancy grid as an image
				 * @param &pos relative pose
				 */
				bool sendMapImage(Grid::pose &pos);

				/**
				* Send Ranges
				* @param &beam_count number of entries in "beams" 
				* @param &beams pointer to table of TDistance pointers
				**/
				bool sendRanges(uint32_t beam_count, const MaCI::Ranging::TDistance **beams);				
};


#endif

